#include "offboard.hpp"



void writeCoordinateToFile_det(const Coordinate_txt& coord) {  
    std::ofstream outfile("/home/orangepi/target_coord.txt", std::ios::app); 
    if (outfile.is_open()) {  
        outfile << coord.x << " " << coord.y << " " << coord.z << std::endl; // 写入数据并换行  
        outfile.close();  
    } else {  
        std::cerr << "Unable to open file for writing:/target_coord.txt" << std::endl;  
    }  
}
void writeCoordinateToFile_ekf(const Coordinate_txt& coord) {  
    std::ofstream outfile("/home/orangepi/target_ekf_coord.txt", std::ios::app); 
    if (outfile.is_open()) {  
        outfile << coord.x << " " << coord.y << " " << coord.z << std::endl; // 写入数据并换行  
        outfile.close();  
    } else {  
        std::cerr << "Unable to open file for writing:/target_ekf_coord.txt" << std::endl;  
    }  
}
void writeCoordinateToFile_predict(const Coordinate_txt& coord) {  
    std::ofstream outfile("/home/orangepi/target_predict_coord.txt", std::ios::app); 
    if (outfile.is_open()) {  
        outfile << coord.x << " " << coord.y << " " << coord.z << std::endl; // 写入数据并换行  
        outfile.close();  
    } else {  
        std::cerr << "Unable to open file for writing:/target_predict_coord.txt" << std::endl;  
    }  
}


int main(int argc, char **argv)
{  
    ros::init(argc, argv, "off_board_test1");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    //read parameters from launch file
    nh.param<bool>("use_sitl",use_sitl,false);
    nh.param<double>("max_vel_beishu",max_vel_beishu,1.0);
    
    //1080p (1920 x 1080) @ 30 fps
    //720p (1280 x 720) @ 30 fps
    nh.param<int>("image_width",image_width,960);
    nh.param<int>("image_height",image_height,720);

    nh.param<double>("P_pos",P_pos,1.0);
    nh.param<double>("I_pos",I_pos,0.0);
    nh.param<double>("D_pos",D_pos,0.0);
    nh.param<double>("I_zone",I_zone,1.0);

    //imu_img_delay delta between imu time and img estimate time,
    nh.param<double>("imu_img_delay",imu_img_delay,0);
    nh.param<double>("track_dist",track_dist,5.0);
    nh.param<bool>("attack_air",attack_air,true);

    
    nh.param<double>("camera_angle",camera_angle,0);
    nh.param<double>("net_offset",net_offset,0);
    
    nh.param<double>("REALSIZE",REALSIZE,0.4);
    // nh.param<double>("FOCUS",FOCUS,1000);
    nh.param<double>("FOCUS_X",FOCUS_X,500);
    nh.param<double>("FOCUS_Y",FOCUS_Y,500);

    double pitch_angle = -45*deg2rad;
    R_up<<cos(pitch_angle),0,sin(pitch_angle),
            0,1,0,
            -sin(pitch_angle),0,cos(pitch_angle);
    // 初始化欧拉角，这里以Z-Y-X顺序（即YPR顺序）为例
    Eigen::Vector3d euler_angles(-90*deg2rad, 0, -45*deg2rad);

    // 将欧拉角转换为旋转矩阵
    rotation_matrix = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) // 绕Z轴旋转
    * Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) // 绕Y轴旋转
    * Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitX()); // 绕X轴旋转        

    //Subscriber
    altitude_sub = nh.subscribe("/mavros/altitude",10,altitude_callback);
    velocitysub = nh.subscribe("/mavros/local_position/velocity_local",10,velocity_callback);
    local_odom_sub = nh.subscribe("/mavros/local_position/odom",10,local_odom_callback);
    

    imu_sub = nh.subscribe("/mavros/imu/data", 10, imu_cb);
    
    rcin_sub = nh.subscribe("/mavros/rc/in",10,rcin_callback);//遥控器
    state_sub = nh.subscribe("/mavros/state", 10, state_cb);

    // fisheye_target_sub = nh.subscribe("/intercept/target_fisheye", 10, fisheye_target_cb);
    // horizonal_target_sub = nh.subscribe("/intercept/horizonal_cam", 10, horizonal_target_cb);
    horizonal_target_sub = nh.subscribe("/yindao/capture_drone", 10, horizonal_target_cb);
    gps_self_sub = nh.subscribe("/mavros/global_position/global", 10, gps_self_cb);
    
    //subscribe from shouchiduan
    shouchi_message_sub = nh.subscribe("/zhou/msg_from_gcs", 10, shouchi_msg_cb);


    
    // gps_target_sub = nh.subscribe("/shouchi/global_position/global", 10, gps_target_cb);
     
    //Publisher
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
    attitude_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);
    marker_pub = nh.advertise<visualization_msgs::Marker>("/target_box", 1);
    attack_state_pub = nh.advertise<std_msgs::Float32MultiArray>("/zhou/attack_state", 1);

    //Service
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");//设置offboard
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("/mavros/setpoint_velocity/mav_frame");//设置frame

    //Timer
    px4_control_timer = nh.createTimer(ros::Duration(0.05), px4_control_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"------------>wait for fcu connected..."<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }
    
    offb_set_mode.request.custom_mode = "OFFBOARD";
    arm_cmd.request.value = true;
    setmavframe.request.mav_frame = 8;// set frame
    last_offboard_request = ros::Time::now();
    if(set_vechile_ned.call(setmavframe) && setmavframe.response.success){
        std::cout<<"-->setmavframe success ..."<<std::endl;}
    else{
        std::cout<<"-->setmavframe failed"<<std::endl;    
    }


    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    // current_point_z = msg.relative;
    current_point.z=  msg.relative;
}

void imu_cb(const sensor_msgs::Imu::ConstPtr& current_msg) { 
    current_angle.x= current_msg->orientation.x;
    current_angle.y= current_msg->orientation.y;
    current_angle.z= current_msg->orientation.z;
    current_angle.w= current_msg->orientation.w;
    //curr_plane_angle roll pitch yaw (x,y,z)
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg->orientation.w, current_msg->orientation.x,
                                        current_msg->orientation.y, current_msg->orientation.z);
    if(!current_state.armed&&abs(curr_plane_angle.x)<1.22&&abs(curr_plane_angle.y)<1.22)
    {
        target_set_angle = current_angle;
        target_set_angle_flag = true;
        ROS_INFO_THROTTLE(0.5, "set target angle: (%.2f, %.2f, %.2f) ", 
                    curr_plane_angle.x*180/3.14159, curr_plane_angle.y*180/3.14159, 
                    curr_plane_angle.z*180/3.14159);
    }
}

void local_odom_callback(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_point.x= current_msg->pose.pose.position.x;
    current_point.y= current_msg->pose.pose.position.y;
    dist_to_home_xy = sqrt(current_point.x*current_point.x+current_point.y*current_point.y);
    // current_point.z= current_msg->pose.pose.position.z;
    // current_point.z= current_point_z;

    // current_angle.x= current_msg->pose.pose.orientation.x;
    // current_angle.y= current_msg->pose.pose.orientation.y;
    // current_angle.z= current_msg->pose.pose.orientation.z;
    // current_angle.w= current_msg->pose.pose.orientation.w;
    // curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    // current_angle_Q = Eigen::Quaterniond(current_msg->pose.pose.orientation.w, current_msg->pose.pose.orientation.x,
    //                                     current_msg->pose.pose.orientation.y, current_msg->pose.pose.orientation.z);
    // R drone

    local_odom_flag = true;
}
void gps_self_cb(const sensor_msgs::NavSatFix &msg)
{
    self_gps_pos.latitude = msg.latitude;
    self_gps_pos.longitude = msg.longitude;
    self_gps_pos.altitude = msg.altitude;
}
void shouchi_msg_cb(const geometry_msgs::PoseStamped &msg)
{
    cam_roll = msg.pose.orientation.x; 
    cam_pitch = msg.pose.orientation.y;
    cam_yaw = msg.pose.orientation.z;
    cam_offset_x = msg.pose.position.x;
    cam_offset_y = msg.pose.position.y;
    proximate_dist = msg.pose.orientation.w;
    cmd1 = msg.pose.position.z;

    ROS_INFO_THROTTLE(0.5, "receive msg from shouchi: (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %d) ", 
                    cam_roll, cam_pitch, 
                    cam_yaw, cam_offset_x, cam_offset_y,
                    proximate_dist, cmd1);
    std::string numStr = std::to_string(cmd1);
    // 在前面补零直到达到指定位数
    while (numStr.length() < 4) {
        numStr = "0" + numStr;
    }
    std::vector<int> cmd_gpio_temp;
    for (int i = 0; i < numStr.length(); i++) {
        cmd_gpio_temp.push_back(numStr[i] - '0');
    }
    cmd_gpio = cmd_gpio_temp;
    // for(int kk=0;kk<cmd_gpio.size();kk++)
    // {
    //     std::cout<<"==>cmd_gpio("<<kk+1<<"): "<<cmd_gpio[kk]<<std::endl;
    // }

    target_gps_receive = ros::Time::now().toSec();
}




void vel_command_send(geometry_msgs::Point vel, double yaw_rate)
{
    vel_command_cnt++;
    geometry_msgs::Vector3 linear_temp;
    geometry_msgs::Vector3 angular_temp;

    geometry_msgs::TwistStamped msg_vel_pub;
    linear_temp.x = vel.x;//forward +
    linear_temp.y = vel.y;//left + roll_vel
    linear_temp.z = vel.z;//up +
    
    angular_temp.x = 0;
    angular_temp.y = 0; 
    angular_temp.z = yaw_rate;
    msg_vel_pub.header.stamp = ros::Time::now();
    msg_vel_pub.header.seq=vel_command_cnt;
    msg_vel_pub.twist.linear=linear_temp;
    msg_vel_pub.twist.angular=angular_temp;

    local_vel_pub.publish(msg_vel_pub);
}


void changeFSMExecState(FSM_STATE new_state, std::string pos_call) 
{
    int    pre_s        = int(exec_state);
    exec_state         = new_state;
    std::cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << std::endl;
}


void printFSMExecState() {
    std::cout << "state: " + state_str[int(exec_state)] << std::endl;
}


//limit speed
geometry_msgs::Point limit_velocity_xyz(float vx, float vy, float vz,float maximum)
{
	geometry_msgs::Point vel;
    
    float velocity = sqrt(vx * vx + vy * vy + vz * vz);
    if(maximum <= 0)
    {
        vel.x = 0;
        vel.y = 0;
        vel.z = 0;
    }
    if(velocity <= maximum)
    {
        vel.x = vx;
        vel.y = vy;
        vel.z = vz;
    }
    //if velocity is bigger than maximum, then limit the vx and vy, and keep the direction at meanwhile.
    else
    {
        //the velocity must not be zero when in this step
        vel.x = vx / velocity * maximum;
        vel.y = vy / velocity * maximum;
        vel.z = vz / velocity * maximum;
    }
    
	return vel;
}

// 基于视觉信息计算目标位置和方向
void calculateTargetAcceleration(Eigen::Vector3d& accel, double& yaw_des) {
    
    // 计算摄像头旋转矩阵
    Eigen::Matrix3d R_c = eulerToRotationMatrix(cam_roll, cam_pitch, cam_yaw);
    
    //  计算归一化图像坐标 (从中心指向目标)
    // double u = k*cam_offset_x / (1920/2);  // 水平偏移
    // double v = k*cam_offset_y / (1080/2);  // 垂直偏移
    double focal_length = 600;
    double u = proximate_dist*cam_offset_x / focal_length;  // 水平偏移
    double v = proximate_dist*cam_offset_y / focal_length;  // 垂直偏移
    double max_vel_temp = std::min(vel_xyz+2,max_vel_beishu);
    double u_lim = limit(u, max_vel_temp/2.0);
    double v_lim = limit(v, max_vel_temp/2.0);
    
 
    // Eigen::Vector3d s(u, v, 1.0);
    Eigen::Vector3d s(max_vel_temp, u_lim, v_lim);

    
    //  转换到世界坐标系
    accel = R_c * s;
    
    //  设置偏航角 (保持当前航向)
    yaw_des = cam_yaw;
    
    ROS_INFO_THROTTLE(0.1, "Target offset: (%.1f, %.1f) | Away direction: (%.2f, %.2f, %.2f) | Accel: (%.2f, %.2f, %.2f)", 
                     cam_offset_x, cam_offset_y, 
                     s.x(), s.y(), s.z(),
                     accel.x(), accel.y(), accel.z());
}



//bias transform to unit:meter
void position_pid_control(Eigen::Vector3d target_position, float velocity_limit, float set_yaw_angle)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim=0.0, vy_lim=0.0, vz_lim=0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, vz = 0;

    geometry_msgs::Point error_pos;
    float dt = ros::Time::now().toSec()-last_calc_time;
    if(dt>1.0) //dt too long maybe need init dt
    {
        last_calc_time = ros::Time::now().toSec();
        return;
    }
    float error_x = (target_position(0)-current_point.x);
    float error_y = (target_position(1)-current_point.y);
    float error_z = (target_position(2)-current_point.z);
    //calculate velocity
    vx = P_pos*error_x-D_pos * current_vel.x+feedforward_x;
    vy = P_pos*error_y-D_pos * current_vel.y+feedforward_y;
    vz = P_pos*error_z-D_pos * current_vel.z+feedforward_z;

    vx_lim = limit_velocity_xyz(vx, vy, vz, velocity_limit).x;
    vy_lim = limit_velocity_xyz(vx, vy, vz, velocity_limit).y;
    vz_lim = limit_velocity_xyz(vx, vy, vz, velocity_limit).z;

    //yaw control
    float current_yaw = curr_plane_angle.z;
    //Drone turns at the smallest angle
    float error_yaw = (set_yaw_angle - current_yaw) * rad2deg;
    if (error_yaw < -180)
        error_yaw += 360;
    else if (error_yaw > 180)
        error_yaw -= 360;
    yaw_rate_p = P_yaw * error_yaw;
    yaw_rate = yaw_rate_p ;
    if (abs(error_yaw) >= YAW_RATE_LIMIT) {
        yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
    } 

    velocity_expected.x = vx_lim * cos(curr_plane_angle.z) + vy_lim * sin(curr_plane_angle.z);
    velocity_expected.y = vy_lim * cos(curr_plane_angle.z) - vx_lim * sin(curr_plane_angle.z);
    velocity_expected.z = vz_lim;
    
    attitude_expect.z = yaw_rate*deg2rad;
    last_calc_time = ros::Time::now().toSec();
}

void position_pid_control_new(Eigen::Vector3d target_position, float velocity_limit, float set_yaw_angle)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim=0.0, vy_lim=0.0, vz_lim=0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, vz = 0;

    geometry_msgs::Point error_pos;
    
    Eigen::Vector3d error_vec;
    error_vec(0) = (target_position(0)-current_point.x);
    error_vec(1) = (target_position(1)-current_point.y);
    error_vec(2) = (target_position(2)-current_point.z);
    double dist = error_vec.norm();
    if(dist<5.0)
    {
        attack_success = 1;
    }
    Eigen::Vector3d error_vec_body;
    error_vec_body(0) = error_vec(0) * cos(curr_plane_angle.z) + error_vec(1) * sin(curr_plane_angle.z);
    error_vec_body(1) = error_vec(1) * cos(curr_plane_angle.z) - error_vec(0) * sin(curr_plane_angle.z);
    error_vec_body(2) = error_vec(2);
    // error_vec.normalize();
    // error_vec *= velocity_limit;
    // std::cout<<"+++>error_vec: "<<error_vec_body(0)<<","<<error_vec_body(1)<<","<<error_vec_body(2)<<std::endl;
    //calculate velocity
    // vx = P_pos*error_vec(0)-D_pos * current_vel.x+feedforward_x;
    // vy = P_pos*error_vec(1)-D_pos * current_vel.y+feedforward_y;
    // vz = P_pos*error_vec(2)-D_pos * current_vel.z+feedforward_z;
    vx = error_vec_body(0)+max_vel_beishu;
    vy = error_vec_body(1);
    vz = error_vec_body(2);

    vx_lim = limit(vx, velocity_limit);

    float max_vel_temp_y = std::min(vel_xyz+2,5.0);
    vy_lim = limit(vy, max_vel_temp_y);
    float z_limit = 5.0;
    vz_lim = limit(vz, z_limit);
    // vx_lim = limit_velocity_xyz(vx, vy, vz, velocity_limit).x;
    // vy_lim = limit_velocity_xyz(vx, vy, vz, velocity_limit).y;
    // vz_lim = limit_velocity_xyz(vx, vy, vz, velocity_limit).z;


    //yaw control
    float current_yaw = curr_plane_angle.z;
    //Drone turns at the smallest angle
    float error_yaw = (set_yaw_angle - current_yaw) * rad2deg;
    if (error_yaw < -180)
        error_yaw += 360;
    else if (error_yaw > 180)
        error_yaw -= 360;
    yaw_rate_p = P_yaw * error_yaw;
    yaw_rate = yaw_rate_p ;
    if (abs(error_yaw) >= YAW_RATE_LIMIT) {
        yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
    } 

    velocity_expected.x = vx_lim;// * cos(curr_plane_angle.z) + vy_lim * sin(curr_plane_angle.z);
    velocity_expected.y = vy_lim;// * cos(curr_plane_angle.z) - vx_lim * sin(curr_plane_angle.z);
    velocity_expected.z = vz_lim;
    
    if(dist>5)
    {
        attitude_expect.z = yaw_rate*deg2rad;
        velocity_expected.y = 0;
    }
    else
    {
        attitude_expect.z = 0.0;
    }
    
    last_calc_time = ros::Time::now().toSec();
    std::cout<<"--->vel calc body : "<<velocity_expected.x<<","
                    <<velocity_expected.y<<","<<velocity_expected.z<<","<<attitude_expect.z<<std::endl;

}

void set_cruise_state()
{
    velocity_expected.x = 0.0;
    velocity_expected.y = 0.0;
    velocity_expected.z = 0.5;
    vel_command_send(velocity_expected, 0);
  
}

void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(0.5);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw    
}



void horizonal_target_cb(const swarm_msgs::BoundingBoxes &msg)
{
    if(ros::Time::now().toSec()-fisheye_update_time<0.5)
    {
        // std::cout<<"------>fisheye target detecting,skip horizonal detect.."<<std::endl;
        return;
    }
    //视觉伺服
    dt_vision = ros::Time::now().toSec()-last_vision_time;
    if(dt_vision>1.0) //dt too long maybe need init dt
    {
        last_vision_time = ros::Time::now().toSec();
        return;
    }
    last_vision_time = ros::Time::now().toSec();
    //计算框大小
    if(msg.bounding_boxes.size()>0)
    {
        Eigen::Vector3d min_weight_vector;
        double real_dist = 0.0;
        for(int i=0;i<msg.bounding_boxes.size();i++)
        {
            box_h = msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin;
            box_v = msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin;
            double box_size = sqrt(box_h * box_h + box_v * box_v);
            if(box_size<5) //筛选太小的框
            {
                continue;
            }
            Bias_H = image_width/2 -(msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0 ; 
            Bias_V = image_height/2 - (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0;
    
            double distance = REALSIZE * sqrt(Bias_H*Bias_H + Bias_V*Bias_V + FOCUS_X*FOCUS_Y) /  box_size;
            // std::cout<<"--------------------------------distance->"<<distance<<std::endl;
            double alpha = atan2(Bias_V,FOCUS_Y);
            double beta = atan2(Bias_H,FOCUS_X);
            // std::cout<<"===========>>self angle: "<<beta*180/3.14<<","<<alpha*180/3.14<<std::endl;
            P_Cam(0) = distance * cos(alpha) * cos(beta);
            P_Cam(1) = distance * cos(alpha) * sin(beta);
            P_Cam(2) = distance * sin(alpha);
            
            // std::cout<<"---horizonal cam_point-> "<<P_Cam(0)<<","<<P_Cam(1)<<","<<P_Cam(2)<<std::endl;
            // R drone
            Eigen::Matrix3d rotation_vehicle = current_angle_Q.toRotationMatrix();  
            //camera setup angle
            Eigen::Matrix3d R_cam_drone = eulerToRotationMatrix(-90*deg2rad, -45*deg2rad, 0);
            P_Cam = R_cam_drone * P_Cam;
            P_Cam(0) += 0.1;
            P_Cam(2) += 0.1;
            // std::cout<<"-------drone cam_point  : "<<P_Cam(0)<<","<<P_Cam(1)<<","<<P_Cam(2)<<std::endl;
            P_Cam = rotation_vehicle * P_Cam;
            // P_Cam(2) -= 0.1;
            
            //netting height offset
            P_Cam(2) += net_offset; 
            
            
        }
        min_weight_vector = P_Cam;
        min_weight_vector(0) +=current_point.x;
        min_weight_vector(1) +=current_point.y;
        min_weight_vector(2) +=current_point.z;
        //draw current target box
        Box vision_box;
        vision_box.x_min = min_weight_vector(0)-0.3;
        vision_box.y_min = min_weight_vector(1)-0.3;
        vision_box.z_min = min_weight_vector(2)-0.3;
        vision_box.x_max = min_weight_vector(0)+0.3;
        vision_box.y_max = min_weight_vector(1)+0.3;
        vision_box.z_max = min_weight_vector(2)+0.3;
        vision_box.id = 0;
        draw_box(vision_box, 105, 0.2, 1, 0, 0);
        P_Cam_min_jiajiao = min_weight_vector;

        error_yaw_esitmate = atan2((P_Cam_min_jiajiao(1)-current_point.y),(P_Cam_min_jiajiao(0)-current_point.x));
        // std::cout<<"-horizonal target_point-> "<<min_weight_vector(0)<<","<<min_weight_vector(1)<<","<<min_weight_vector(2)<<std::endl;
        if(vel_xyz>1.0)
        {
            if(real_dist>10)
            {
                ekf_predict_time = 0.5;
            }
            else
            {
                ekf_predict_time =0.1 + real_dist/vel_xyz;
            }
            
        }
        else
        {
            ekf_predict_time = 0.2;
        }
        // ekf_predict_time = 0.2;
        ekf_predict_process();

        have_target_time = ros::Time::now().toSec();
    }

}
// void fisheye_target_cb(const swarm_msgs::BoundingBoxes &msg)
// {
//     //视觉伺服
//     dt_vision = ros::Time::now().toSec()-last_vision_time;
//     if(dt_vision>1.0) //dt too long maybe need init dt
//     {
//         last_vision_time = ros::Time::now().toSec();
//         return;
//     }
//     last_vision_time = ros::Time::now().toSec();
//     //计算框大小
//     if(msg.bounding_boxes.size()>0)
//     {
//         Eigen::Vector3d min_weight_vector;
//         double real_dist = 0.0;
//         for(int i=0;i<msg.bounding_boxes.size();i++)
//         {
//             double newx1 = (msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0 - (20+(960/2)); 
//             double newy1 = (-50+720/2) - (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0;
//             double center_angle = -atan2(newy1, newx1);  //left rotation is + ,diverse it
//             // std::cout<<"----center_angle: "<<center_angle*rad2deg<<std::endl;
//             double dist_to_camera_center = sqrt(newx1*newx1+newy1*newy1);
//             // std::cout<<"----dist_to_camera_center: "<<dist_to_camera_center<<std::endl;
//             double pitch_deg = 90.0-dist_to_camera_center*0.25;
//             // std::cout<<"====>pitch_deg: "<<pitch_deg<<std::endl; 
//             double pitch_to_camera = pitch_deg*deg2rad;
//             //calc box size
//             double yolo_box_size = sqrt((msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin)*(msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin)
//                                         +(msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin)*(msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin));
//             // std::cout<<"----yolo_box_size: "<<yolo_box_size<<std::endl;
//             double virtual_focus = 195.0;
//             real_dist = (REALSIZE*virtual_focus)/yolo_box_size;

//             // std::cout<<"====>real_dist_fisheye: "<<real_dist<<std::endl; 
//             P_Cam(0) = real_dist*cos(pitch_to_camera)/sqrt(1+tan(center_angle)*tan(center_angle));
//             P_Cam(1) = real_dist*cos(pitch_to_camera)*tan(center_angle)/sqrt(1+tan(center_angle)*tan(center_angle));
//             P_Cam(2) = real_dist*sin(pitch_to_camera);
            
//             if(abs(center_angle)>90.0*deg2rad)
//             {
//                 P_Cam(0) = -P_Cam(0);
//                 P_Cam(1) = -P_Cam(1);
//             }
//             // std::cout<<"---cam_point_fisheye-> "<<P_Cam(0)<<","<<P_Cam(1)<<","<<P_Cam(2)<<std::endl;
//             // R drone
//             Eigen::Matrix3d rotation_vehicle = current_angle_Q.toRotationMatrix();  
//             //camera setup angle
//             P_Cam = rotation_matrix * P_Cam;
//             P_Cam = rotation_vehicle * P_Cam;
//             // std::cout<<"---cam_point_fisheye local-> "<<P_Cam(0)<<","<<P_Cam(1)<<","<<P_Cam(2)<<std::endl;
//             //netting height offset
              // P_Cam(2) += net_offset; 
            
            
//         }
//         min_weight_vector = P_Cam;
//         min_weight_vector(0) +=current_point.x;
//         min_weight_vector(1) +=current_point.y;
//         min_weight_vector(2) +=current_point.z;
//         //draw current target box
//         Box vision_box;
//         vision_box.x_min = min_weight_vector(0)-0.2;
//         vision_box.y_min = min_weight_vector(1)-0.2;
//         vision_box.z_min = min_weight_vector(2)-0.2;
//         vision_box.x_max = min_weight_vector(0)+0.2;
//         vision_box.y_max = min_weight_vector(1)+0.2;
//         vision_box.z_max = min_weight_vector(2)+0.2;
//         vision_box.id = 0;
//         draw_box(vision_box, 105, 0.2, 1, 0, 0);
//         P_Cam_min_jiajiao = min_weight_vector;

//         error_yaw_esitmate = atan2((P_Cam_min_jiajiao(1)-current_point.y),(P_Cam_min_jiajiao(0)-current_point.x));
//         // std::cout<<"-target_point-> "<<min_weight_vector(0)<<","<<min_weight_vector(1)<<","<<min_weight_vector(2)<<std::endl;
//         // if(vel_xyz>1.0)
//         // {
//         //     if(real_dist>10)
//         //     {
//         //         ekf_predict_time = 0.2;
//         //     }
//         //     else
//         //     {
//         //         ekf_predict_time = real_dist/vel_xyz;
//         //     }
            
//         // }
//         // else
//         // {
//         //     ekf_predict_time = 0.2;
//         // }
//         ekf_predict_time = 0.2; 
//         ekf_predict_process();
//         fisheye_update_time = ros::Time::now().toSec();
//         have_target_time = ros::Time::now().toSec();
//     }
// }

void ekf_predict_process()
{
    // Record Target Position
    Coordinate_txt coord2 = {P_Cam_min_jiajiao(0), P_Cam_min_jiajiao(1), P_Cam_min_jiajiao(2)}; // 示例坐标  
        writeCoordinateToFile_det(coord2);

    // EKF2
    z_measured << P_Cam_min_jiajiao(0),P_Cam_min_jiajiao(1),P_Cam_min_jiajiao(2); // 赋值  
    ekf.set_dt(dt_vision);
    ekf.predict();
    ekf.update(z_measured); 
    Eigen::VectorXd x_ = ekf.getState();
    P_Cam_Ekf(0) = x_[0];
    P_Cam_Ekf(1) = x_[1];
    P_Cam_Ekf(2) = x_[2];
    // Record EKF2 Result
    coord2 = {P_Cam_Ekf(0), P_Cam_Ekf(1), P_Cam_Ekf(2)}; // 示例坐标 
        writeCoordinateToFile_ekf(coord2);
    feedforward_x = x_[3];
    feedforward_y = x_[4];
    feedforward_z = x_[5];
    // Predict with EKF,  t = dist_to_target/self_vel
    //std::cout<<"--->ekf_predict_time: "<<ekf_predict_time<<std::endl;
    P_Cam_Latency_Eliminate(0) = P_Cam_Ekf(0)+x_[3]*ekf_predict_time+0.5*x_[6]*ekf_predict_time*ekf_predict_time;
    P_Cam_Latency_Eliminate(1) = P_Cam_Ekf(1)+x_[4]*ekf_predict_time+0.5*x_[7]*ekf_predict_time*ekf_predict_time;
    P_Cam_Latency_Eliminate(2) = P_Cam_Ekf(2);//+x_[5]*ekf_predict_time+0.5*x_[8]*ekf_predict_time*ekf_predict_time;
    // Record predicted Result
    coord2 = {P_Cam_Latency_Eliminate(0), P_Cam_Latency_Eliminate(1), P_Cam_Latency_Eliminate(2)}; // 示例坐标 
    writeCoordinateToFile_predict(coord2);
    // draw result of ekf and predict
    // ekf
    Box vision_box3;
    vision_box3.x_min = P_Cam_Latency_Eliminate(0)-0.3;
    vision_box3.y_min = P_Cam_Latency_Eliminate(1)-0.3;
    vision_box3.z_min = P_Cam_Latency_Eliminate(2)-0.3;
    vision_box3.x_max = P_Cam_Latency_Eliminate(0)+0.3;
    vision_box3.y_max = P_Cam_Latency_Eliminate(1)+0.3;
    vision_box3.z_max = P_Cam_Latency_Eliminate(2)+0.3;
    vision_box3.id = 201;
    draw_box(vision_box3, 201, 0.2, 0.0, 0.0, 1.0);


    // // desire where to track
    // Eigen::Vector3d target_track_vector;
    // target_track_vector(0) = P_Cam_Latency_Eliminate(0)-current_point.x;
    // target_track_vector(1) = P_Cam_Latency_Eliminate(1)-current_point.y;
    // target_track_vector(2) = P_Cam_Latency_Eliminate(2)-current_point.z;
    // double horizon_yaw_angle = atan2(target_track_vector(1),target_track_vector(0));
    // double vertical_pitch_angle = atan2(target_track_vector(2),sqrt(target_track_vector(0)*target_track_vector(0)+target_track_vector(1)*target_track_vector(1)));
    // P_Cam_Track(0) = P_Cam_Latency_Eliminate(0)-track_dist*cos(horizon_yaw_angle);
    // P_Cam_Track(1) = P_Cam_Latency_Eliminate(1)-track_dist*sin(horizon_yaw_angle);
    // //need test +-
    // P_Cam_Track(2) = P_Cam_Latency_Eliminate(2)-track_dist*sin(vertical_pitch_angle);  
    // Box vision_box4;
    // vision_box4.x_min = P_Cam_Track(0)-0.2;
    // vision_box4.y_min = P_Cam_Track(1)-0.2;
    // vision_box4.z_min = P_Cam_Track(2)-0.2;
    // vision_box4.x_max = P_Cam_Track(0)+0.2;
    // vision_box4.y_max = P_Cam_Track(1)+0.2;
    // vision_box4.z_max = P_Cam_Track(2)+0.2;
    // vision_box4.id = 302;
    // draw_box(vision_box4, 301, 0.2, 0.5, 0.0, 0.5);
    // std::cout<<"--track_point-> "<<P_Cam_Track(0)<<","<<P_Cam_Track(1)<<","<<P_Cam_Track(2)<<std::endl;
}
void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel1_value = rcvalue.channels[0]; //roll
    channel2_value = rcvalue.channels[1]; //pitch
    channel3_value = rcvalue.channels[2]; //thrust
    channel4_value = rcvalue.channels[3]; //yaw
    channel5_value = rcvalue.channels[4]; //model position att stable
    channel6_value = rcvalue.channels[5]; //kill switch
    channel7_value = rcvalue.channels[6]; //A button
    channel8_value = rcvalue.channels[7]; //B button
    channel_test = rcvalue.channels[9];
    thrust_test = 0.5+((channel_test-1050)/(900.0*(1.0/0.5)));
}

void velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vel.x = msg.twist.linear.x;
    current_vel.y = msg.twist.linear.y;
    current_vel.z = msg.twist.linear.z;
    vel_xyz = sqrt(current_vel.x*current_vel.x+current_vel.y*current_vel.y);
}

geometry_msgs::TwistStamped generateVelocityCmd() {
    geometry_msgs::TwistStamped cmd;
    cmd.header.stamp = ros::Time::now();
    cmd.header.frame_id = "world"; // 使用世界坐标系
    
    // 初始化速度指令
    cmd.twist.linear.x = 0.0;
    cmd.twist.linear.y = 0.0;
    cmd.twist.linear.z = 0.0;
    cmd.twist.angular.x = 0.0;
    cmd.twist.angular.y = 0.0;
    cmd.twist.angular.z = 0.0;
    // 1. 计算目标加速度指令 (沿着远离方向)
    Eigen::Vector3d a_des;
    double yaw_des;
    calculateTargetAcceleration(a_des, yaw_des);
    
    // 2. 将加速度转换为速度指令（比例控制）
    // 注意：在速度控制中，我们直接控制速度而不是加速度
    // 使用简单的比例控制：速度 = 增益 * 加速度

    double a_p = 1.0;
    double vx = a_des.x()*a_p;
    double vy = a_des.y()*a_p;
    double vz = a_des.z()*a_p;

    double max_vel_temp = std::min(vel_xyz+2,max_vel_beishu);

    double vx_lim = limit(vx, max_vel_temp);
    double vy_lim = limit(vy, max_vel_temp);
    double vz_lim = limit(vz, max_vel_temp);


    cmd.twist.linear.x = vx_lim * cos(curr_plane_angle.z) + vy_lim * sin(curr_plane_angle.z);
    cmd.twist.linear.y = vy_lim * cos(curr_plane_angle.z) - vx_lim * sin(curr_plane_angle.z);
    cmd.twist.linear.z = vz_lim;

    
    // 5. 偏航控制 (保持当前航向)
    double yaw_error = yaw_des - curr_plane_angle.z;
    // 归一化角度误差到[-π, π]
    while (yaw_error > M_PI) yaw_error -= 2*M_PI;
    while (yaw_error < -M_PI) yaw_error += 2*M_PI;
    double yaw_rate = yaw_error*1.0;
    double yaw_rate_limit = limit(yaw_rate, 1.0);
    cmd.twist.angular.z = yaw_rate_limit;
    
    ROS_INFO_THROTTLE(0.1, "Velocity Cmd: (%.2f, %.2f, %.2f) | Yaw: %.2f", 
                        cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z,
                        cmd.twist.angular.z);
    return cmd;
}

// 欧拉角转旋转矩阵
Eigen::Matrix3d eulerToRotationMatrix(double roll, double pitch, double yaw) {
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    
    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q.matrix();
}

void px4_control_callback(const ros::TimerEvent& /*event*/) 
{
    // printFSMExecState();
    if(print_state_cnt>100)
    {
        //print state per second
        printFSMExecState();
        std::cout<<"--->gpio 1: "<<cmd_gpio[0]<<std::endl;
        std::cout<<"--->gpio 2: "<<cmd_gpio[1]<<std::endl;
        std::cout<<"--->gpio 3: "<<cmd_gpio[2]<<std::endl;
        std::cout<<"--->gpio 4: "<<cmd_gpio[3]<<std::endl;
        print_state_cnt = 0;
    }
    print_state_cnt++;
    std_msgs::Float32MultiArray state_pub_msg;

    //test temp 
    // position_pid_control_new(P_Cam_Latency_Eliminate, 10, error_yaw_esitmate);
    // geometry_msgs::TwistStamped cmd_test = generateVelocityCmd();
    switch (exec_state) { 
    case INIT: {
        // cmd1 = 1;
        // std::string numStr = std::to_string(cmd1);
        // // 在前面补零直到达到指定位数
        // while (numStr.length() < 4) {
        //     numStr = "0" + numStr;
        // }
        // std::vector<int> cmd_gpio_temp;
        // for (int i = 0; i < numStr.length(); i++) {
        //     cmd_gpio_temp.push_back(numStr[i] - '0');
        // }
        // cmd_gpio = cmd_gpio_temp;
        // for(int kk=0;kk<cmd_gpio.size();kk++)
        // {
        //     std::cout<<"==>cmd_gpio("<<kk+1<<"): "<<cmd_gpio[kk]<<std::endl;
        // }
        state_pub_msg.data.push_back(1);
        if (!local_odom_flag) {
            break;
        }
        // // 发送初始指令
        // mavros_msgs::AttitudeTarget init_cmd;
        // init_cmd.header.stamp = ros::Time::now();
        // init_cmd.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;

        // // 设置角速率命令（在机体坐标系中）
        // init_cmd.body_rate.x = 0.0;           // 滚转角速率设为0
        // init_cmd.body_rate.y = 0.0;           // 俯仰角速率命令
        // init_cmd.body_rate.z = 0.0;           // 偏航角速率设为0
                

        // init_cmd.thrust = 0.0;
        // // init_cmd.thrust = thrust_test;
        
        
        // attitude_pub.publish(init_cmd);

        // if(channel5_value>1800&&cmd1>999)
        // {
        //     changeFSMExecState(TAKEOFF, "offboard_fsm");
        // }
        changeFSMExecState(CRUISE, "offboard_fsm");
        
        
    break;
    }
    
    case CRUISE: {
        state_pub_msg.data.push_back(1);
        set_cruise_state();
        if(current_state.mode == "OFFBOARD")
        {
            if(cmd_gpio[0]==1)
            {
                if(!current_state.armed)
                {
                    changeFSMExecState(TAKEOFF, "offboard_fsm"); 
                }
                else
                {
                    changeFSMExecState(ATTACK, "offboard_fsm");
                }   
                
            }
            else 
            {
                if(current_state.armed)
                {
                    changeFSMExecState(ATTACK, "offboard_fsm"); 
                }
                
            }
             
        }
    break;
    }
    case TAKEOFF: {
        state_pub_msg.data.push_back(1);
        if(target_set_angle_flag)
        {
            if( !current_state.armed &&
            (ros::Time::now() - last_offboard_request > ros::Duration(1.0))&&current_state.mode == "OFFBOARD"){
            takeoff_time = ros::Time::now().toSec();
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed...");
            }
            last_offboard_request = ros::Time::now();
            }
            //速度模式起飞
            // velocity_expected.x = 0.0;
            // velocity_expected.y = 0.0;
            // velocity_expected.z = 1.0;
            // vel_command_send(velocity_expected, 0);
            // if(current_point.z>2.0)
            // {
            //     changeFSMExecState(ATTACK, "offboard_fsm");
            // }

            //角速率模式起飞
            
            
            // 发送初始指令
            mavros_msgs::AttitudeTarget init_cmd;
            init_cmd.header.stamp = ros::Time::now();
            init_cmd.type_mask = 0;// mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;

            init_cmd.orientation = target_set_angle;

            // 设置角速率命令（在机体坐标系中）
            init_cmd.body_rate.x = 0.0;           // 滚转角速率设为0
            init_cmd.body_rate.y = 0.0;           // 俯仰角速率命令
            init_cmd.body_rate.z = 0.0;           // 偏航角速率设为0
                    

            init_cmd.thrust = 0.9;
            // init_cmd.thrust = thrust_test;
            
            
            attitude_pub.publish(init_cmd);
        }
        

        if(dist_to_home_xy>15)
        {
            changeFSMExecState(ATTACK, "offboard_fsm"); 
        }

 

    break;
    }
    case ATTACK: {
        
        if(ros::Time::now().toSec()-have_target_time<0.5&&cmd_gpio[3]==1)
        {
            double max_vel_temp = std::min(vel_xyz+2,max_vel_beishu);
            // position_pid_control(P_Cam_Track, max_vel_temp, error_yaw_esitmate);
            if(P_Cam_Latency_Eliminate.allFinite())
            {
                if(P_Cam_Latency_Eliminate(2)>200)
                {
                    P_Cam_Latency_Eliminate(2) = 200;
                }

                position_pid_control_new(P_Cam_Latency_Eliminate, max_vel_temp, error_yaw_esitmate);
            }
            else
            {
                if(P_Cam_min_jiajiao(2)>200)
                {
                    P_Cam_min_jiajiao(2) = 200;
                }
                std::cout<<"======ekf failed , use origin position!"<<std::endl;
                position_pid_control_new(P_Cam_min_jiajiao, max_vel_temp, error_yaw_esitmate);
            }
            vel_command_send(velocity_expected, attitude_expect.z);
            state_pub_msg.data.push_back(2);
            // std::cout<<"--->vel send: "<<velocity_expected.x<<","<<velocity_expected.y<<","<<velocity_expected.z<<","<<attitude_expect.z<<std::endl;
            target_gps_receive = 0;
        }
        else
        {
            if(ros::Time::now().toSec()-target_gps_receive<0.5)
            {
                cmd_send = generateVelocityCmd();
                // if(current_point.z<5.0)
                // {
                //     cmd_send.twist.linear.z = 4.0-current_point.z;
                // }
                local_vel_pub.publish(cmd_send);
            }
            else if(ros::Time::now().toSec()-target_gps_receive<10.0)
            {
                // if(current_point.z<5.0)
                // {
                //     cmd_send.twist.linear.z = 4.0-current_point.z;
                // }
                local_vel_pub.publish(cmd_send);
            }
            else
            {
                set_cruise_state();
            }
            state_pub_msg.data.push_back(1);
        }
        
        if(cmd_gpio[2]==1)
        {
            changeFSMExecState(CANCEL, "offboard_fsm"); 
        }
    break;
    }
    case CANCEL:{
        std::cout<<"------>cancel mission,back to home....."<<std::endl;
         // 切换到返航模式
        mavros_msgs::SetMode rtl_set_mode;
        rtl_set_mode.request.custom_mode = "AUTO.RTL";
        
        // 条件检查：无人机已解锁、当前模式不是返航模式、且距离上次请求超过1秒
        if(current_state.armed && 
           current_state.mode != "AUTO.RTL" && 
           (ros::Time::now() - last_offboard_request > ros::Duration(1.0)))
        {
            if(set_mode_client.call(rtl_set_mode) && rtl_set_mode.response.mode_sent){
                ROS_INFO("Return mode (RTL) enabled");
            }
            last_offboard_request = ros::Time::now();
        }


    break;
    }

    }
    state_pub_msg.data.push_back(P_Cam_min_jiajiao(0));
    state_pub_msg.data.push_back(P_Cam_min_jiajiao(1));
    state_pub_msg.data.push_back(P_Cam_min_jiajiao(2));
    state_pub_msg.data.push_back(current_point.z);
    state_pub_msg.data.push_back(curr_plane_angle.z);
    state_pub_msg.data.push_back(attack_success);
    attack_state_pub.publish(state_pub_msg);
}

